import math
import numpy as np

# ==== 已知参数 ====
FY1 = (17800.0, 0.0, 1800.0)  # 无人机 FY1 初始位置
T = (0.0, 200.0, 0.0)  # 真目标中心（圆柱体下底面圆心）
M1 = (20000.0, 0.0, 2000.0)  # 导弹 M1 初始位置
v_uav = 120.0  # UAV 速度 (m/s)
t_drop = 1.5  # 投放时间 (s)
fuse = 3.6  # 引信延时 (s)
g = 9.8  # 重力加速度 (m/s^2)
cloud_r = 10.0  # 云团半径 (m)
cloud_sink = 3.0  # 云团下沉速度 (m/s)
v_missile = 300.0  # 导弹速度 (m/s)
cylinder_r = 7.0  # 圆柱体半径 (m)
cylinder_h = 10.0  # 圆柱体高度 (m)

# ==== 定义圆柱体上的8个代表点 ====
# 下底面圆周上的4个点
points_bottom = [
    (T[0] + cylinder_r, T[1], T[2]),  # 右侧点
    (T[0], T[1] + cylinder_r, T[2]),  # 前方点
    (T[0] - cylinder_r, T[1], T[2]),  # 左侧点
    (T[0], T[1] - cylinder_r, T[2])  # 后方点
]

# 上底面圆周上的4个点
points_top = [
    (T[0] + cylinder_r, T[1], T[2] + cylinder_h),  # 右侧点
    (T[0], T[1] + cylinder_r, T[2] + cylinder_h),  # 前方点
    (T[0] - cylinder_r, T[1], T[2] + cylinder_h),  # 左侧点
    (T[0], T[1] - cylinder_r, T[2] + cylinder_h)  # 后方点
]

# 合并所有代表点
target_points = points_bottom + points_top

# ==== UAV 飞向原点的水平单位向量 ====
hx, hy = -FY1[0], -FY1[1]
h_norm = math.hypot(hx, hy)
ux, uy = hx / h_norm, hy / h_norm

# ==== 起爆时刻与位置 ====
t_explosion = t_drop + fuse
C0 = (
    FY1[0] + v_uav * ux * t_explosion,  # x
    FY1[1] + v_uav * uy * t_explosion,  # y
    FY1[2] - 0.5 * g * fuse ** 2  # z
)

# ==== 导弹速度向量 ====
# 导弹飞向假目标原点 (0,0,0)，而不是真目标
to_dummy_target = (-M1[0], -M1[1], -M1[2])
norm = math.sqrt(sum(c * c for c in to_dummy_target))
v_m = tuple(c / norm * v_missile for c in to_dummy_target)

# ==== 采样计算遮蔽区间 ====
dt = 0.01  # 时间步长 (s)
shield_flags = []
times = [i * dt for i in range(int(20 / dt) + 1)]

for tau in times:
    # 导弹位置 (全局时间 t = t_explosion + tau)
    t_global = t_explosion + tau
    Mx = M1[0] + v_m[0] * t_global
    My = M1[1] + v_m[1] * t_global
    Mz = M1[2] + v_m[2] * t_global

    # 云团中心
    Cx, Cy, Cz = C0[0], C0[1], C0[2] - cloud_sink * tau

    # 检查所有8个代表点是否都被遮蔽
    all_obstructed = True

    for point in target_points:
        Px, Py, Pz = point

        # 计算导弹到目标点的向量
        vx, vy, vz = Px - Mx, Py - My, Pz - Mz
        seg_len2 = vx * vx + vy * vy + vz * vz

        if seg_len2 == 0:
            dist2 = (Cx - Mx) ** 2 + (Cy - My) ** 2 + (Cz - Mz) ** 2
        else:
            # 计算云团中心到线段的最近点
            wx, wy, wz = Cx - Mx, Cy - My, Cz - Mz
            u = (wx * vx + wy * vy + wz * vz) / seg_len2
            u = max(0, min(1, u))  # 限制在线段范围内

            # 最近点坐标
            px, py, pz = Mx + u * vx, My + u * vy, Mz + u * vz

            # 计算最近距离的平方
            dist2 = (Cx - px) ** 2 + (Cy - py) ** 2 + (Cz - pz) ** 2

        # 如果任意一个点没有被遮蔽，则整个目标没有被遮蔽
        if dist2 > cloud_r ** 2:
            all_obstructed = False
            break

    shield_flags.append(all_obstructed)

# ==== 提取连续遮蔽区间 ====
intervals, total_time = [], 0.0
i = 0
while i < len(times):
    if shield_flags[i]:
        j = i
        while j + 1 < len(times) and shield_flags[j + 1]:
            j += 1
        start, end = times[i], times[j] + dt
        intervals.append((start, end))
        total_time += end - start
        i = j + 1
    else:
        i += 1

# ==== 输出结果 ====
print("爆炸时间:", t_explosion, "s")
print("爆炸位置 C0:", C0)
print("有效遮蔽区间 (爆炸后的时间):", intervals)
print("总有效遮蔽时长:", total_time, "s")